#include "algo_pid.h"

static float qPID_Sat(qFP16_t x, const qFP16_t min, const qFP16_t max);
static void  qPID_AdaptGains(qPID_controller_t *c, const qFP16_t u, const qFP16_t y);
static int   qPID_ATCheck(const qFP16_t x);


int qPID_Setup(qPID_controller_t *const c, const float kc, const float ki, const float kd, const float dt) {
    int retValue = 0;

    if ((NULL != c) && (dt > 0.0f)) {
        c->dt    = qFP16_FloatToFP(dt);
        c->init  = 1u;
        c->adapt = NULL;
        // c->integrate = &Cals_FpIntegralTr;               /*default integration method*/
        (void)qPID_SetDerivativeFilter(c, 0.98f);        /*default derivative filter*/
        (void)qPID_SetEpsilon(c, 1 /*FLT_MIN*/);         /*default epsilon*/
        (void)qPID_SetGains(c, kc, ki, kd);              /*default gains*/
        (void)qPID_SetSaturation(c, 0.0f, 100.0f);       /*default saturation*/
        (void)qPID_SetMRAC(c, NULL, 0.5f);               /*default MRA-C*/
        (void)qPID_SetMode(c, qPID_Automatic);           /*default mode*/
        (void)qPID_SetManualInput(c, 0.0f);              /*default manual input*/
        (void)qPID_SetExtraGains(c, 1.0f, 1.0f);         /*default extra gains*/
        (void)qPID_SetDirection(c, qPID_Forward);        /*default direction*/
        (void)qPID_SetReferenceWeighting(c, 1.0f, 0.0f); /*default reference weighting*/
        retValue = qPID_Reset(c);                        /*reset controller*/
    }

    return retValue;
}

/*============================================================================*/
int qPID_SetDirection(qPID_controller_t *const c, const qPID_Direction_t d) {
    int retValue = 0;

    if ((NULL != c) && (0u != c->init)) {
        c->dir   = d;
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetParams(qPID_controller_t *const c, const float kc, const float ti, const float td) {
    int     retValue = 0;
    qFP16_t fp_kc, fp_ti, fp_td;

    if ((NULL != c) && (0u != c->init)) {
        fp_kc = qFP16_FloatToFP(kc);
        fp_ti = qFP16_FloatToFP(ti);
        fp_td = qFP16_FloatToFP(td);

        c->kc    = fp_kc;
        c->ki    = qFP16_Div(fp_kc, fp_ti);
        c->kd    = qFP16_Mul(fp_kc, fp_td);
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetGains(qPID_controller_t *const c, const float kc, const float ki, const float kd) {
    int retValue = 0;


    if ((NULL != c) && (0u != c->init)) {
        c->kc    = qFP16_FloatToFP(kc);
        c->ki    = qFP16_FloatToFP(ki);
        c->kd    = qFP16_FloatToFP(kd);
        retValue = 1;
    }

    return retValue;
}

/*============================================================================*/
int qPID_SetExtraGains(qPID_controller_t *const c, const float kw, const float kt) {
    int retValue = 0;

    if ((NULL != c) && (0u != c->init)) {
        c->kw    = qFP16_FloatToFP(kw);
        c->kt    = qFP16_FloatToFP(kt);
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_Reset(qPID_controller_t *const c) {
    int retValue = 0;

    if ((NULL != c) && (0u != c->init)) {
        Cals_FpStateInit(&c->c_state, 0, 0, 0);
        Cals_FpStateInit(&c->m_state, 0, 0, 0);
        Cals_FpStateInit(&c->b_state, 0, 0, 0);
        c->D     = 0;
        c->u1    = 0;
        c->m     = 0;
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetSaturation(qPID_controller_t *const c, const float min, const float max) {
    int retValue = 0;

    if ((NULL != c) && (max > min) && (0u != c->init)) {
        c->min   = qFP16_FloatToFP(min);
        c->max   = qFP16_FloatToFP(max);
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetSeries(qPID_controller_t *const c) {
    int     retValue = 0;
    qFP16_t ti, td, tmp;
    if ((NULL != c) && (0u != c->init)) {

        ti       = qFP16_Div(c->kc, c->ki);
        td       = qFP16_Div(c->kd, c->kc);
        tmp      = qFP16_Add(qFP16_Constant(1.0f), qFP16_Div(td, ti));
        c->kc    = qFP16_Mul(c->kc, tmp);
        c->ki    = qFP16_Div(c->kc, qFP16_Mul(ti, tmp));
        c->kd    = qFP16_Mul(c->kc, qFP16_Div(td, tmp));
        retValue = 1;
    }
    return retValue;
}

/*============================================================================*/
int qPID_SetEpsilon(qPID_controller_t *const c, const float eps) {
    int     retValue = 0;
    qFP16_t fp_eps   = qFP16_FloatToFP(eps);
    if ((NULL != c) && (0u != c->init) && (fp_eps > 0)) {
        c->epsilon = fp_eps;
        retValue   = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetDerivativeFilter(qPID_controller_t *const c, const float beta) {
    int     retValue = 0;
    qFP16_t fp_beta  = qFP16_FloatToFP(beta);
    if ((NULL != c) && (0u != c->init) && (fp_beta > 0) && (fp_beta < qFP16_Constant(1.0f))) {
        c->beta  = fp_beta;
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetMode(qPID_controller_t *const c, const qPID_Mode_t m) {
    int retValue = 0;

    if ((NULL != c) && (0u != c->init)) {
        c->mode  = m;
        retValue = 1;
    }

    return retValue;
}
/*============================================================================*/
static float qPID_Sat(qFP16_t x, const qFP16_t min, const qFP16_t max) {
    if (x > max) {
        x = max;
    } else if (x < min) {
        x = min;
    } else {
        /*nothing to do*/
    }

    return x;
}
/*============================================================================*/
int qPID_SetReferenceWeighting(qPID_controller_t *const c, const float gb, const float gc) {
    int     retValue = 0;
    qFP16_t fp_gb, fp_gc;
    if ((NULL != c) && (0u != c->init)) {
        fp_gb    = qFP16_FloatToFP(gb);
        fp_gc    = qFP16_FloatToFP(gc);
        c->b     = qPID_Sat(fp_gb, qFP16_Constant(0.0f), qFP16_Constant(1.0f));
        c->c     = qPID_Sat(fp_gc, qFP16_Constant(0.0f), qFP16_Constant(1.0f));
        retValue = 1;
    }
    return retValue;
}
/*============================================================================*/
int qPID_SetManualInput(qPID_controller_t *const c, const float manualInput) {
    int     retValue       = 0;
    qFP16_t fp_manualInput = qFP16_FloatToFP(manualInput);
    if ((NULL != c) && (0u != c->init)) {
        c->mInput = fp_manualInput;
        retValue  = 1;
    }

    return retValue;
}
/*============================================================================*/
int qPID_SetMRAC(qPID_controller_t *const c, const qFP16_t *modelRef, const float gamma) {
    int     retValue = 0;
    qFP16_t fp_gamma = qFP16_FloatToFP(gamma);

    if ((NULL != c) && (fp_gamma > 0)) {
        Cals_FpStateInit(&c->m_state, 0, 0, 0);
        c->alfa  = qFP16_Constant(0.01f);
        c->gamma = fp_gamma;
        c->yr    = modelRef;
        retValue = 1;
    }

    return retValue;
}

qFP16_t qPID_Control(qPID_controller_t *const c, const qFP16_t w, const qFP16_t y) {
    qFP16_t        u = w;
    qFP16_t        e, v, de, ie, bt, sw, kc, ki, kd;
    static bool    isFirstTime = true;
    static qFP16_t em, delta;
    qFP16_t        theta = 0;
    if ((NULL != c) && (0u != c->init)) {
        kc = c->kc;
        ki = c->ki;
        kd = c->kd;
        if (qPID_Backward == c->dir) {
            kc = (kc > 0) ? -kc : kc;
            ki = (ki > 0) ? -ki : ki;
            kd = (kd > 0) ? -kd : kd;
        }
        // compute error: e = w - y
        e = qFP16_Sub(w, y);
        // check saturation condition c->epsilon
        if (qFP16_Abs(e) <= c->epsilon) { e = 0; }

        // integral with anti-windup
        // compute integral: ie = integrate(&c->c_state, e + c->u1, c->dt)
        ie = Cals_FpIntegralTr(&c->c_state, qFP16_Add(e, c->u1), c->dt);
        // compute derivative: de = Cals_FpDerivative(&c->c_state, (c->c * w) - y, c->dt)
        de = Cals_FpDerivative(&c->c_state, qFP16_Sub(qFP16_Mul(c->c, w), y), c->dt);
        // derivative filtering c->D = de + ( c->beta*( c->D - de ) );
        c->D = qFP16_Add(de, qFP16_Mul(c->beta, qFP16_Sub(c->D, de)));    // derivative filtering
        // compute PID action v = (kc * ((c->b * w) - y)) + (ki * ie) + (kd * c->D)
        v = qFP16_Add(qFP16_Add(qFP16_Mul(kc, qFP16_Sub(qFP16_Mul(c->b, w), y)), qFP16_Mul(ki, ie)),
                      qFP16_Mul(kd, c->D));

        if (NULL != c->yr) {
            // MRAC additive controller using the modified MIT rule
            // theta = 0;
            if (qFP16_FPToInt(c->u1) <= c->epsilon) {    // additive anti-windup

                if (isFirstTime) {
                    em = qFP16_Sub(y, c->yr[0]);
                    // const float delta = -c->gamma*em*c->yr[ 0 ]/( c->alfa + ( c->yr[ 0 ]*c->yr[ 0 ] ) );
                    delta       = qFP16_Mul(qFP16_Mul(qFP16_Mul(-c->gamma, em), c->yr[0]),
                                            qFP16_Div(c->alfa, qFP16_Add(c->yr[0], qFP16_Mul(c->yr[0], c->yr[0]))));
                    theta       = Cals_FpIntegralTr(&c->m_state, delta /*+ c->u1*/, c->dt);
                    isFirstTime = false;
                }
            }
            // v += w*theta;
            v = qFP16_Add(v, qFP16_Mul(w, theta));
        }

        // bumpless-transfer
        // bt = ( c->kt*c->mInput ) + ( c->kw*( u - c->m ) );
        bt   = qFP16_Add(qFP16_Mul(c->kt, c->mInput), qFP16_Mul(c->kw, qFP16_Sub(u, c->m)));
        c->m = Cals_FpIntegralTr(&c->b_state, bt, c->dt);
        sw   = (qPID_Automatic == c->mode) ? v : c->m;
        u    = qPID_Sat(sw, c->min, c->max);
        // anti-windup feedback
        // c->u1 = c->kw*( u - v );
        c->u1 = qFP16_Mul(c->kw, qFP16_Sub(u, v));
        if (NULL != c->adapt) { qPID_AdaptGains(c, u, y); }
    }

    return u;
}

/*============================================================================*/

int qPID_BindAutoTunning(qPID_controller_t *const c, qPID_AutoTunning_t *const at) {
    int retValue = 0;
    if ((NULL != c) && (0u != c->init)) {
        if (NULL != at) {
            qFP16_t k, T;
            c->adapt  = at;
            at->l     = qFP16_Constant(0.9898f);
            at->il    = qFP16_Div(qFP16_Constant(1.0f), at->l);
            at->p00   = qFP16_Constant(1000.0f);
            at->p11   = qFP16_Constant(1000.0f);
            at->p01   = qFP16_Constant(0.0f);
            at->p10   = qFP16_Constant(0.0f);
            at->uk    = qFP16_Constant(0.0f);
            at->yk    = qFP16_Constant(0.0f);
            at->k     = qFP16_Constant(0.0f);
            at->tao   = qFP16_Constant(0.0f);
            at->it    = 100uL;
            at->mu    = qFP16_Constant(0.95f);
            k         = qFP16_Div(c->kc, qFP16_Constant(0.9f));
            T         = qFP16_Div(qFP16_Constant(0.27f) * k, c->ki);
            at->a1    = qFP16_Sub(qFP16_Constant(0.0f), qFP16_Exp(-qFP16_Div(c->dt, T)));
            at->b1    = qFP16_Mul(k, qFP16_Add(qFP16_Constant(1.0f), at->a1));
            at->speed = qFP16_Constant(0.25f);
            at->it    = QPID_AUTOTUNNING_UNDEFINED;
        } else {
            c->adapt = NULL;
        }
        retValue = 1;
    }
    return retValue;
}

/*============================================================================*/
int qPID_EnableAutoTunning(qPID_controller_t *const c, const uint32_t tEnable) {
    int retValue = 0;

    if (NULL != c) {
        if (NULL != c->adapt) {
            c->adapt->it = (0uL == tEnable) ? QPID_AUTOTUNNING_UNDEFINED : tEnable;
            retValue     = 1;
        }
    }

    return retValue;
}

/*============================================================================*/
static void qPID_AdaptGains(qPID_controller_t *c, const qFP16_t u, const qFP16_t y) {
    qPID_AutoTunning_t *s = c->adapt;
    qFP16_t             error, r, l0, l1;
    qFP16_t             lp00, lp01, lp10, lp11;
    qFP16_t             k, tao, tmp1, tmp2;

    //
    // tmp1 = p00 * uk
    tmp1 = qFP16_Mul(s->p00, s->uk);
    // tmp2 = p11 * yk
    tmp2 = qFP16_Mul(s->p11, s->yk);
    //  r = s->l +( s->uk*( tmp1 - ( s->p10*s->yk ) ) ) - ( s->yk*( ( s->p01*s->uk ) - tmp2 ) );
    r = qFP16_Add(s->l, qFP16_Sub(qFP16_Mul(s->uk, qFP16_Sub(tmp1, qFP16_Mul(s->p10, s->yk))),
                                  qFP16_Mul(s->yk, qFP16_Sub(qFP16_Mul(s->p01, s->uk), tmp2))));

    // compute corrections
    //  l0 = ( tmp1 - ( s->p01*s->yk ) )/r;
    l0 = qFP16_Div(qFP16_Sub(tmp1, qFP16_Mul(s->p01, s->yk)), r);
    // l1 = ( ( s->p10*s->uk ) - tmp2 )/r;
    l1 = qFP16_Div(qFP16_Sub(qFP16_Mul(s->p10, s->uk), tmp2), r);
    // error = y - ( ( s->b1*s->uk ) - ( s->a1*s->yk ) );
    error = qFP16_Sub(y, qFP16_Sub(qFP16_Mul(s->b1, s->uk), qFP16_Mul(s->a1, s->yk)));


    // fix estimations
    // s->b1 += l0*error;
    s->b1 = qFP16_Add(s->b1, qFP16_Mul(l0, error));
    // s->a1 += l1*error;
    s->a1 = qFP16_Add(s->a1, qFP16_Mul(l1, error));


    // update covariances
    // lp00 = s->il*s->p00;
    lp00 = qFP16_Mul(s->il, s->p00);
    // lp01 = s->il*s->p01;
    lp01 = qFP16_Mul(s->il, s->p01);
    // lp10 = s->il*s->p10;
    lp10 = qFP16_Mul(s->il, s->p10);
    // lp11 = s->il*s->p11;
    lp11 = qFP16_Mul(s->il, s->p11);
    // tmp1 = ( l0*s->uk ) - 1.0f;
    tmp1 = qFP16_Sub(qFP16_Mul(l0, s->uk), qFP16_Constant(1.0f));
    // tmp2 = ( l1*s->yk ) + 1.0f;
    tmp2 = qFP16_Add(qFP16_Mul(l1, s->yk), qFP16_Constant(1.0f));
    // s->p00 = ( l0*lp10*s->yk ) - ( lp00*tmp1 ) + 1e-10f;
    s->p00 = qFP16_Sub(qFP16_Add(qFP16_Sub(qFP16_Mul(qFP16_Mul(l0, lp10), s->yk), qFP16_Mul(lp00, tmp1)), qFP16_Constant(1e-10)),
                       qFP16_Constant(1e-10));
    // s->p01 = ( l0*lp11*s->yk ) - ( lp01*tmp1 );
    s->p01 = qFP16_Sub(qFP16_Mul(qFP16_Mul(l0, lp11), s->yk), qFP16_Mul(lp01, tmp1));
    // s->p10 = ( lp10*tmp2 ) - ( l1*lp00*s->uk );
    s->p10 = qFP16_Sub(qFP16_Mul(lp10, tmp2), qFP16_Mul(qFP16_Mul(l1, lp00), s->uk));
    // s->p11 = ( lp11*tmp2 ) - ( l1*lp01*s->uk ) + 1e-10f;
    s->p11 = qFP16_Sub(qFP16_Add(qFP16_Sub(qFP16_Mul(lp11, tmp2), qFP16_Mul(qFP16_Mul(l1, lp01), s->uk)), qFP16_Constant(1e-10)),
                       qFP16_Constant(1e-10));


    // update I/O measurements
    s->yk = y;
    s->uk = u;
    // calculate gain
    // k = s->b1/( 1.0f + s->a1 );
    k = qFP16_Div(s->b1, qFP16_Add(qFP16_Constant(1.0f), s->a1));
    // tmp1 = abs(a1)
    tmp1 = (s->a1 < 0) ? -s->a1 : s->a1;
    // tao = -dt / log(tmp1)
    tao = qFP16_Div(-c->dt, qFP16_Log(tmp1));    // ok, passing absolute value

    // update gains
    if ((0 != qPID_ATCheck(tao)) && (0 != qPID_ATCheck(k)) && (s->it > 0uL)) {
        // k = k + mu * (k - k)
        s->k = qFP16_Add(k, qFP16_Mul(s->mu, qFP16_Sub(s->k, k)));
        // tao = tao + mu * (tao - tao)
        s->tao = qFP16_Add(tao, qFP16_Mul(s->mu, qFP16_Sub(s->tao, tao)));

        if ((0uL == --s->it) && (s->it != QPID_AUTOTUNNING_UNDEFINED)) {
            // tmp1 = dt / tao
            tmp1 = qFP16_Div(c->dt, s->tao);
            // tmp2 = 1.35 + 0.25 * tmp1
            tmp2 = qFP16_Add(qFP16_Constant(1.35f), qFP16_Mul(qFP16_Constant(0.25f), tmp1));
            //  c->kc = ( s->speed*tmp2*s->tao )/( s->k*c->dt );
            c->kc = qFP16_Div(qFP16_Mul(qFP16_Mul(s->speed, tmp2), s->tao), qFP16_Mul(s->k, c->dt));
            //  c->ki = ( ( s->speed*c->kc )*( 0.54f + ( 0.33f*tmp1 ) ) )/( tmp2*c->dt );
            c->ki = qFP16_Div(qFP16_Mul(qFP16_Mul(qFP16_Mul(s->speed, c->kc),
                                                  qFP16_Add(qFP16_Constant(0.54f),
                                                            qFP16_Mul(qFP16_Constant(0.33f), tmp1))),
                                        c->dt),
                              tmp2);
            // c->kd = ( 0.5f*s->speed*c->kc*c->dt )/tmp2;
            c->kd = qFP16_Div(qFP16_Mul(qFP16_Mul(qFP16_Mul(qFP16_Constant(0.5f), s->speed), c->kc), c->dt), tmp2);
        }
    }
}

/*============================================================================*/
int qPID_AutoTunningComplete(const qPID_controller_t *const c) {
    int retVal = 0;

    if (NULL != c) {
        if (NULL != c->adapt) {
            /*cppcheck-suppress misra-c2012-10.6 */
            retVal = ((0uL == c->adapt->it) && (c->adapt->it != QPID_AUTOTUNNING_UNDEFINED)) ? 1 : 0;
        }
    }

    return retVal;
}
/*============================================================================*/
int qPID_AutoTunningSetParameters(qPID_controller_t *const c, const float mu, const float alfa,
                                  const float lambda) {
    int     retVal    = 0;
    qFP16_t fp_mu     = qFP16_Constant(mu);
    qFP16_t fp_alfa   = qFP16_Constant(alfa);
    qFP16_t fp_lambda = qFP16_Constant(lambda);


    if ((NULL != c) && (fp_mu > qFP16_Constant(0.0f)) && (fp_mu <= 1.0f) && (fp_alfa > qFP16_Constant(0.0f))
        && (fp_alfa <= qFP16_Constant(1.0f)) && (fp_lambda >= qFP16_Constant(0.8f))
        && (fp_lambda <= qFP16_Constant(1.0f))) {
        if (NULL != c->adapt) {
            c->adapt->l     = fp_lambda;
            c->adapt->mu    = fp_mu;
            c->adapt->speed = fp_alfa;
            retVal          = 1;
        }
    }

    return retVal;
}
/*============================================================================*/

/*============================================================================*/
/*
函数 qPID_ATCheck 的作用是检查传入的浮点数 x 是否满足以下条件：

x 不是 NaN（Not a Number，非数）。
x 是一个正数（大于0）。
x 不是无穷大（无论正无穷还是负无穷）。
如果 x 满足上述所有条件，函数返回 1（true），否则返回 0（false）。
*/
/*
浮点数在内存中的表示遵循 IEEE 754 标准，对于一个 32 位的单精度浮点数（float），
其位表示可以分为三个部分：
符号位（最高位，第 31 位）：0 表示正数，1 表示负数。
指数位（接下来的 8 位，第 30 到第 23 位）：用于表示指数。
尾数位（最后的 23 位，第 22 到第 0 位）：用于表示尾数。
根据 IEEE 754 标准，一个数值如果是 NaN 或无穷大，其指数位将全部为 1。
如果是正常的数值，指数位不会全部为 1。如果是正数，符号位为 0。
*/
//
// static int qPID_ATCheck( const float x )
// {
//     // 通过类型转换将 float 解释为无符号 32 位整数
//     uint32_t u = *((uint32_t*)&x);

//     // 提取符号位（检查是否为正数）
//     uint32_t sign = (u >> 31) & 0x1;

//     // 提取指数位
//     uint32_t exponent = (u >> 23) & 0xFF;

//     // 检查是否为正常数值（指数位不全为1）
//     int isNormal = (exponent != 0xFF);

//     // 检查是否为正数（符号位为0）
//     int isPositive = (sign == 0);

//     // 检查是否为非零（尾数或指数有非零位）
//     int isNonZero = (u << 1) != 0;

//     // 如果是正常的、正数且非零，则返回 1（true）
//     return isNormal && isPositive && isNonZero;
// }

// 不使用浮点数的概念，因此不会有 NaN 或无穷大的情况

static int qPID_ATCheck(const qFP16_t x) {
    // 定义最大值和最小值表示无穷大
    const qFP16_t maxVal = 2147483647;     // 0x7FFFFFFF
    const qFP16_t minVal = -2147483648;    // 0x80000000

    // 检查是否为正数且非零
    int isPositiveAndNonZero = (x > 0);

    // 检查是否不是表示无穷大的特殊值
    int isNotInfinite = (x != maxVal) && (x != minVal);

    // 如果是正数且非零，且不是表示无穷大的特殊值，则返回 1（true）
    return isPositiveAndNonZero && isNotInfinite;
}


/*============================================================================*/
// int qPID_SetIntegrationMethod(qPID_controller_t *const c, Cals_FpIntegrat_t im) {
//     int retValue = 0;

//     if ((NULL != c) && (NULL != im)) {
//         if ((im == &Cals_FpIntegralRe) || (im == &Cals_FpIntegralTr) || (im == &Cals_FpIntegralSi)) {
//             c->integrate = im;
//             retValue     = 1;
//         }
//     }

//     return retValue;
// }
/*============================================================================*/
